﻿using System;
using System.Windows.Forms;
using System.IO;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using OpenCvSharp;

namespace testyOpenCV
{
	public class ImageColor
	{
		public static IplImage lImg;
		public static IplImage rImg;
		public static IplImage someImg;
		public static int dmax;
		public static int recH;
		public static int recW;
		public static int wartMed;
		//***************************************************************************************************


		public static CvScalar pixL = new CvScalar();
		public static CvScalar pixR = new CvScalar();
		public int control = 1;// = 255 / dmax;
		//public static int dmax = 15;   // max serching disp
		//static int recW = 7;    // szerokosc dopasowania
		//static int recH = 3;    // wysokosc dopasowania
		//public static int dmax = Image.dmaxIpl;   // max serching disp
		//static int recW = Image.winWidth;    // szerokosc dopasowania
		//static int recH = Image.winHeight;    // wysokosc dopasowania
		//static int wartMed = Image.medWin; // wielkosc okna filtru medianowego
		int LGT = 10;   // Line Growing Treshold
		int alfa = 1;
		int foc = 15;
		float m = 6;   // length between cameras
		int maxM, maxN;   // image resolution 
		public uint top = 0;
		public int state = 0;
		public int tempI;
		public int tempJ;
		public int tempIk;
		public int tempJw;
		public float errorE;    // energy error
		public IplImage dispIm = someImg;
		public IplImage depthMapIm = someImg;
		public static IplImage dispMedIm = someImg;
		//public static IplImage lIm = lImg.Clone();
		//public static IplImage rIm = rImg.Clone();

		public uint topLam = 0;
		public float vE = 0;
		//***************************************************************************************************
		//konstruktor pobieranych zdjec lewego i prawego
		public ImageColor(string fileNameL, string fileNameR, int dmaxIplT, int winHeightT, int winWidthT, int medWinT)
		{
			dmax = dmaxIplT;
			recH = winHeightT;
			recW = winWidthT;
			wartMed = medWinT;
			int recCentW = (recW / 2) + 1;
			int recNCentW = (recW / 2);
			int recCentH = (recH / 2) + 1;
			int recNCentH = (recH / 2);
			try
			{
				lImg = new IplImage(fileNameL);
				rImg = new IplImage(fileNameR);
			}
			catch (FileNotFoundException ex)
			{
				MessageBox.Show("Błędna nazwa pliku.", "Błąd");
			}

			//************************************************************************************************
			IplImage dispIm = Cv.CreateImage(rImg.Size, BitDepth.U8, 1);
			Cv.CvtColor(rImg, dispIm, ColorConversion.BgrToGray);  
					
			IplImage dispMedIm = Cv.CreateImage(rImg.Size, BitDepth.U8, 1);
      Cv.CvtColor(rImg, dispMedIm, ColorConversion.BgrToGray);


      maxM = lImg.Height;
      maxN = lImg.Width;
      uint[,] bL = new uint[maxM, maxN];   //matrix with colour value
      uint[,] gL = new uint[maxM, maxN];
      uint[,] rL = new uint[maxM, maxN];
      uint[,] bR = new uint[maxM, maxN];   //matrix with colour value
      uint[,] gR = new uint[maxM, maxN];
      uint[,] rR = new uint[maxM, maxN];

      int[,] lPix = new int[maxM, maxN];
      int[,] rPix = new int[maxM, maxN];

      int[,] disp = new int[maxM, maxN];
      uint[,] errDisp = new uint[maxM, maxN];
      float[,] edis = new float[maxM, maxN];
      int[,] regMap = new int[maxM, maxN];
      float[,] depthMap = new float[maxM, maxN];

      for (int i = 0; i < maxM; i++)   // loop fill-up a matrix  ???????????????
      {
        for (int j = 0; j < maxN; j++)
        {
          pixL = lImg.Get2D(i, j);
          rL[i, j] = (uint)pixL.Val2;
          gL[i, j] = (uint)pixL.Val1;
          bL[i, j] = (uint)pixL.Val0;
          lPix[i, j] = (int)pixL.Val0;

          pixR = rImg.Get2D(i, j);
          rR[i, j] = (uint)pixR.Val2;
          gR[i, j] = (uint)pixR.Val1;
          bR[i, j] = (uint)pixR.Val0;
          rPix[i, j] = (int)pixR.Val0;

          disp[i, j] = 0;
          dispIm.Set2D(i, j, CvScalar.ScalarAll(0));
        }
      }

      for (int m = (0 + recNCentW); m < (maxM - recNCentW); m++)
      {
        for (int n = (dmax + recNCentW); n < (maxN - recNCentW); n++)
        {
          double prevSum = 10000;
          for (int d = 0; d <= dmax; d++)
          {
            double sum = 0;
            for (int i = (m - recNCentH); i <= (m + recNCentH); i++)
            {
              for (int j = (n - recNCentW); j <= (n + recNCentW); j++)
              {
                uint diffR = ((rL[i, j] - rR[i, j - d]) * (rL[i, j] - rR[i, j - d]))/9;
                uint diffG = ((gL[i, j] - gR[i, j - d]) * (gL[i, j] - gR[i, j - d]))/9;
                uint diffB = ((bL[i, j] - bR[i, j - d]) * (bL[i, j] - bR[i, j - d]))/9;
                uint diff = diffR + diffG + diffB;
                sum += diff;
                tempI = i;
                tempJ = j;

              }
            }
            if (sum < prevSum)
            {
              disp[tempI, tempJ] = d;
              if (d > control) control = d;
              prevSum = sum;
            }
          }
        }
      }

      control = 255 / control;
      for (int m = 0; m < (maxM); m++)
      {
        for (int n = 0; n < (maxN); n++)
        {
          int a = disp[m, n];
          int temp = control * a;
          dispIm.Set2D(m, n, CvScalar.ScalarAll(temp));
					//disp[m, n] = temp;
        }
      }

      Cv.Smooth(dispIm, dispMedIm, SmoothType.Median, wartMed, wartMed);

      dispMedIm.SaveImage("result_medianRGB.png");
			dispIm.SaveImage("resultRGB.png");

      //using (CvWindow window_DispImg = new CvWindow("DepthMapColor", dispIm))
      using (CvWindow window_DispMedImg = new CvWindow("DepthMedMapColor", dispMedIm))
      {
        CvWindow.WaitKey();
      }

			//************************************************************************************************

		}
		public ImageColor(string fileName)
		{
			try
			{
				someImg = new IplImage(fileName);
			}
			catch (FileNotFoundException ex2)
			{
			}

		}
	}
}

/* Pomiar czasu

DateTime startTime = DateTime.Now;
 
//np. jakies sortowanie
 
DateTime stopTime = DateTime.Now;
TimeSpan roznica = stopTime - startTime;
Console.WriteLine("Czas pracy:" + roznica.TotalMilliseconds);
*/
